import serial
import struct
import time
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
from geometry_msgs.msg import PoseStamped
from rclpy.qos import qos_profile_sensor_data
import sys
import math

# 定义报文头和数据ID
HEAD = 0xA5
CRC16_POLY = 0x8005
EXPECTED_LENGTH = 61  # 预计的报文长度
EARTH_RADIUS = 6371008.7714150598
                    
def main(args=None):
    rclpy.init(args=args)
    node = PublisherNode()
    port = ('/dev/ttyACM'+str(sys.argv[1]))
    print(port)
    node.read_and_parse_serial_data(port)
    
    
if __name__ == "__main__":
    main()